﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.ComponentModel;
using Microsoft.Kinect;
using System.Windows;
using System.Threading;
using System.Runtime.InteropServices;
using System.Net.Sockets;
using System.Net;

namespace kinectSocketController
{
    class kinectSocketController
    {
        private KinectSensor sensor = null;
        private CoordinateMapper coordinateMapper = null;
        private BodyFrameReader bodyFrameReader = null;
        private Body[] bodies = null;
        private CameraSpacePoint handrLast = new CameraSpacePoint();
        private CameraSpacePoint handlLast = new CameraSpacePoint();
        private CameraSpacePoint point2send = new CameraSpacePoint();
        private Client client;
        static int frameCount = 0;
        private static bool exitFlag = false;

        public kinectSocketController()
        {
            Console.WriteLine("启动中。。。");
            this.sensor = KinectSensor.GetDefault();
            this.coordinateMapper = this.sensor.CoordinateMapper;
            this.sensor.Open();
            Console.WriteLine("连接设备。。。");
            this.bodyFrameReader = this.sensor.BodyFrameSource.OpenReader();
            Console.WriteLine("kinect就绪。。。");
            client = new Client();
            if (this.bodyFrameReader != null)
            {
                this.bodyFrameReader.FrameArrived += this.Reader_FrameArrived;
            }

        }
        static void Main(string[] args)
        {

            kinectSocketController P = new kinectSocketController();
            while (exitFlag == false)
            {
            }
            P.sensor.Close();
            
            // Open a reader and subscribe to frame arrival events this.bodyReader = this.sensor.BodySource.OpenReader(); this.bodyReader.FrameArrived += BodyFrameArrived;

        }

        private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
        {
            bool dataReceived = false;
            using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
            {
                if (bodyFrame != null)
                {
                    if (this.bodies == null)
                    {
                        this.bodies = new Body[bodyFrame.BodyCount];
                    }

                    // The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
                    // As long as those body objects are not disposed and not set to null in the array,
                    // those body objects will be re-used.
                    bodyFrame.GetAndRefreshBodyData(this.bodies);
                    dataReceived = true;
                }
            }

            if (dataReceived)
            {
                if (frameCount++ > 10)
                {
                    Console.WriteLine(frameCount);
                    frameCount = 0;
                    foreach (Body body in this.bodies)
                    {

                        if (body.Joints[JointType.HandRight].TrackingState != TrackingState.NotTracked)
                        {
                            IReadOnlyDictionary<JointType, Joint> joints = body.Joints;
                            CameraSpacePoint handr = joints[JointType.HandRight].Position;
                            CameraSpacePoint handl = joints[JointType.HandLeft].Position;
                            CameraSpacePoint shoulderl = joints[JointType.ShoulderLeft].Position;
                            HandState handState = body.HandRightState;
                            point2send.X = handr.X - handrLast.X;
                            point2send.Y = handr.Y - handrLast.Y;
                            point2send.Z = handr.Z - handrLast.Z;
                            handlLast = handl;
                            handrLast = handr;
                            CmdObj cmdObj2Send = new CmdObj(point2send.X, point2send.Y, point2send.Z, handState==HandState.Closed);
                            if (handl.Y > shoulderl.Y)
                            {
                                if (handState == HandState.Closed)
                                {
                                    exitFlag = !this.client.send(cmdObj2Send);
                                    Console.WriteLine("X: " + point2send.X + " Y: " + point2send.Y + " Z: " + point2send.Z + " grab");
                                }
                                else
                                {
                                    exitFlag = !this.client.send(cmdObj2Send);
                                    Console.WriteLine("X: " + point2send.X + " Y: " + point2send.Y + " Z: " + point2send.Z + " no");
                                }
                            }
                            break;
                        }
                    }
                }
            }
        }

    }

}

